﻿using ROSBridgeLib;
using ROSBridgeLib.sensor_msgs;
using ROSBridgeLib.std_msgs;
using ROSBridgeLib.geometry_msgs;
using ROSBridgeLib.nav_msgs;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using SimpleJSON;
using UnityEngine.Events;

public class MapSubscriber : BaseSubscriber {


    private Texture2D mapTexture;

    private int lastmapWidth=0;

    private int lastmapheight=0;

  
    public Color32 obstacleColor;

    public Color32 unknowColor;

    public Color32 walkableColor;


    private Color32[] colorsOnMap;

    private bool mapInitilized=false;


    [SerializeField]
    UnityEvent OnFirstGetMap;



    void Start()
    {

        topic="map";

        messageType="nav_msgs/OccupancyGrid";

       
        OnReset();

       

    }

    public override void OnReset()
    {
        base.OnReset();

        mapInitilized=false;



        if (colorsOnMap!=null)
        {
      
        for (int i=0;i<colorsOnMap.Length;i++)
        {

            colorsOnMap[i]=unknowColor;

        }
        }

    }
	

    public override ROSBridgeMsg Parse(JSONNode msg)
    {
       
        return new OccupancyGrid(msg["msg"]);

    }

    public override void OnMessage()
    {


    
        OccupancyGrid ogrid=(OccupancyGrid)GetTopParsedPackage();

        if (ogrid==null)
        {
            
            return;

        }

        MapDisplay(ogrid);

    }



    void UpdateMap(OccupancyGrid map)
    {
        


        int count=map.width*map.height;

        for (int i=0;i<count;i++)
        {



            int chance= map.data[i];

            Color32 pointColor;

            if (chance==-1)
            {

                pointColor=unknowColor;


            }else if (chance==0)
            {

                pointColor=walkableColor;


            }else
            {

                pointColor=obstacleColor;

            }

         
        

            colorsOnMap[i]=pointColor;

        }


        mapTexture.SetPixels32(colorsOnMap);

        mapTexture.Apply(false);


    }



    public void MapDisplay(OccupancyGrid map)
    {


        Debug.Log("Update map");


        bool lastMapInitized=mapInitilized;


        if (lastmapWidth<map.width || lastmapheight<map.height)
        {

            InitializeMap(map);

        }


        UpdateMap(map);


        if (lastMapInitized==false && mapInitilized==true)
        {

            MessageBox.ShowMessage("地图已收到");

            OnFirstGetMap.Invoke();



        }



    }

    void InitializeMap(OccupancyGrid map)
    {


        mapInitilized=true;

        colorsOnMap=new Color32[map.width*map.height];

        mapTexture=new Texture2D(map.width,map.height);

        Sprite sprite= Sprite.Create(mapTexture, new Rect(0.0f, 0.0f,map.width,map.height), new Vector2(0f, 0f), 1.0f/map.resolution);

        GetComponent<SpriteRenderer>().sprite=sprite;

        transform.localPosition=map.info.GetOrigin().position.ToVector3();

    }


   
}
